package go84;

import javax.microedition.location.Coordinates;

public class UTM {
	public static class Datum {
		public double a;
		public double b, f, e2, dx, dy, dz;

		Datum(double a, double invF, double dx, double dy, double dz) {
			this.a = a;
			this.dx = dx;
			this.dy = dy;
			this.dz = dz;
			f = 1 / invF;
			b = a - a / invF;
			e2 = (a * a - b * b) / (a * a);
		}
	}

	public final static double Deg2Rad = Math.PI / 180.0;
	public static Datum ed50 = new Datum(6378388, 297, 103, 106, 141);
	public static Datum grs80 = new Datum(6378300, 293.4660019, -235, -85, 264);

	public final static int LatLon = 3;
	public final static int NewITM = 1;
	public final static int OldITM = 2;
	public final static int UTM = 0;

	public static Datum wgs84 = new Datum(6378137, 298.257223563, 0, 0, 0);

	public static double atan(double x) {
		boolean signChange = false;
		boolean Invert = false;
		int sp = 0;
		double x2, a, xp;
		// check up the sign change
		if (x < 0) {
			x = -x;
			signChange = true;
		}
		// check up the invertation
		if (x > 1) {
			x = 1 / x;
			Invert = true;
		}
		// process shrinking the domain until x<PI/12
		while (x > Math.PI / 12) {
			sp++;
			x = (x * Math.sqrt(3) - 1) / (x + Math.sqrt(3));
		}
		// calculation core
		x2 = x * x;
		/*
		 * a=x2+1.4087812; a=0.55913709/a; a=a+0.60310579; a=a-x2*0.05160454;
		 * a=a*x;
		 */
		xp = x;
		a = 0;
		for (int i = 0; i < 5; i++) {
			a = a + xp / (4 * i + 1);
			xp = xp * x2;
			a = a - xp / (4 * i + 3);
			xp = xp * x2;
		}
		// process until sp=0
		while (sp > 0) {
			a = a + Math.PI / 6;
			sp--;
		}
		// invertation took place
		if (Invert)
			a = Math.PI / 2 - a;
		// sign change took place
		if (signChange)
			a = -a;
		//
		return a;
	}

	public static Coordinates dtmConv(Coordinates old, Datum from, Datum to) {
		return old;
		/*double oldLat, oldLong, oldAlt, lat, lon, alt, merid;
		double oldX, oldY, oldZ, x, y, z, oldV, v;
		 // TODO:
					// NOTICE!!!!!!!!!!!!!!!!!!!@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
		
		 * oldLat=old.getLatitude(); oldLong=old.getLongitude(); if
		 * (!Float.isNaN(old.getAltitude())) { oldAlt=old.getAltitude(); } else
		 * { oldAlt=0; } merid=Math.floor((oldLong)/6)*6+3;
		 * oldV=from.a/Math.sqrt
		 * (1-from.e2*Math.sin(oldLat*Deg2Rad)*Math.sin(oldLat*Deg2Rad));
		 * oldX=(oldV
		 * +oldAlt)*Math.cos(oldLat*Deg2Rad)*Math.cos(oldLong*Deg2Rad);
		 * oldY=(oldV
		 * +oldAlt)*Math.cos(oldLat*Deg2Rad)*Math.sin(oldLong*Deg2Rad);
		 * oldZ=((1-from.e2)*oldV+oldAlt)*Math.sin(oldLat*Deg2Rad);
		 * x=oldX+to.dx-from.dx; y=oldY+to.dy-from.dy; z=oldZ+to.dz-from.dz;
		 * lat=
		 * atan((z+to.e2*oldV*Math.sin(oldLat*Deg2Rad))/Math.sqrt(x*x+y*y))/Deg2Rad
		 * ;
		 * v=to.a/Math.sqrt(1-to.e2*Math.sin(lat*Deg2Rad)*Math.sin(lat*Deg2Rad)
		 * );
		 * lat=atan((z+to.e2*v*Math.sin(lat*Deg2Rad))/Math.sqrt(x*x+y*y))/Deg2Rad
		 * ; lon=atan(y/x)/Deg2Rad;
		 * alt=x/(Math.cos(lat*Deg2Rad)*Math.cos(lon*Deg2Rad))-v; Coordinates
		 * res=new Coordinates(lat, lon, (float)alt); return res;
		 */
	}

	public static int[] latLon2XY(Coordinates point, int datum) {
		Coordinates coord = new Coordinates(0, 0, 0);
		if (datum == UTM) {
			coord = dtmConv(point, wgs84, ed50);
			// coord=point;
		} else if ((datum == NewITM) | (datum == OldITM)) {
			coord = molodensky(point, wgs84, grs80);
		}
		double latitude, longitude;
		double F0, A1 = 0, B1 = 0, K0 = 0, N0 = 0, N1, N2, N3, E2, E0 = 0, L0 = 0, K, L, SINK, COSK, TANK, TANK2, COSK2, COSK3, K3, K4, MeridWest, Merid, J1, J2, J3, J4, J5, J6, J7, J8, J9, M, Temp, V, R, H2, P, P2, P4, North, East;
		int INorth, IEast;
		latitude = coord.getLatitude();
		longitude = coord.getLongitude();
		int[] res = new int[2];
		if (datum == UTM) { // UTM International Ellipsoid
			F0 = 0.9996;
			A1 = 6378388.000 * F0;
			B1 = 6356911.946 * F0;
			K0 = 0;
			N0 = 0;
			E0 = 500000;
		}

		else if (datum == NewITM) {
			F0 = 1.000006700000000; // Local scale factor on Central Meridian
			A1 = 6378300 * F0; // Major Semi-axis, Airy Ellipsoid
			B1 = 6356566 * F0; // Minor Semi-axis, Airy Ellipsoid
			K0 = 31.73439361 * Deg2Rad; // Lat of True Origin
			Merid = 35.20451694;
			// K0=(31+44/60+3.816999999992845/3600)*Deg2Rad; // Lat of True
			// Origin
			// Merid=35+12/60+16.260999999993260/3600;
			L0 = Merid * Deg2Rad; // Long of True Origin (2W)
			N0 = 626907.389999999900000; // Grid Northing of True Origin (m)
			E0 = 219529.584000000000000; // Grid Easting of True Origin (m)
		} else if (datum == OldITM) {
			F0 = 1.000006700000000; // Local scale factor on Central Meridian
			A1 = 6378300 * F0; // Major Semi-axis, Airy Ellipsoid
			B1 = 6356566 * F0; // Minor Semi-axis, Airy Ellipsoid
			K0 = 31.73439361 * Deg2Rad; // Lat of True Origin
			Merid = 35.20451694;
			L0 = Merid * Deg2Rad; // Long of True Origin (2W)
			N0 = 1126907.389999999900000; // Grid Northing of True Origin (m)
			E0 = 169529.584000000000000; // Grid Easting of True Origin (m)
		}
		N1 = (A1 - B1) / (A1 + B1); // n
		N2 = N1 * N1;
		N3 = N2 * N1;
		E2 = ((A1 * A1) - (B1 * B1)) / (A1 * A1); // e^2
		K = latitude * Deg2Rad;
		L = longitude * Deg2Rad;
		SINK = Math.sin(K);
		COSK = Math.cos(K);
		TANK = SINK / COSK;
		TANK2 = TANK * TANK;
		COSK2 = COSK * COSK;
		COSK3 = COSK2 * COSK;
		K3 = K - K0;
		K4 = K + K0;

		if (datum == UTM) {
			Merid = Math.floor((longitude) / 6) * 6 + 3;
			if ((latitude >= 72) && (longitude >= 0)) {
				if (longitude < 9)
					Merid = 3;
				else if (longitude < 21)
					Merid = 15;
				else if (longitude < 33)
					Merid = 27;
				else if (longitude < 42)
					Merid = 39;
			}
			if ((latitude >= 56) && (latitude < 64)) {
				if ((longitude >= 3) && (longitude < 12))
					Merid = 9;
			}
			L0 = Merid * Deg2Rad; // Long of True Origin (3,9,15 etc)
		}

		// ArcofMeridian
		J3 = K3 * (1 + N1 + 1.25 * (N2 + N3));
		J4 = Math.sin(K3) * Math.cos(K4) * (3 * (N1 + N2 + 0.875 * N3));
		J5 = Math.sin(2 * K3) * Math.cos(2 * K4) * (1.875 * (N2 + N3));
		J6 = Math.sin(3 * K3) * Math.cos(3 * K4) * 35 / 24 * N3;
		M = (J3 - J4 + J5 - J6) * B1;

		// VRH2
		Temp = 1 - E2 * SINK * SINK;
		V = A1 / Math.sqrt(Temp);
		R = V * (1 - E2) / Temp;
		H2 = V / R - 1.0;

		P = L - L0;
		P2 = P * P;
		P4 = P2 * P2;
		J3 = M + N0;
		J4 = V / 2 * SINK * COSK;
		J5 = V / 24 * SINK * (COSK3) * (5 - (TANK2) + 9 * H2);
		J6 = V / 720 * SINK * COSK3 * COSK2
				* (61 - 58 * (TANK2) + TANK2 * TANK2);
		North = J3 + P2 * J4 + P4 * J5 + P4 * P2 * J6;
		J7 = V * COSK;
		J8 = V / 6 * COSK3 * (V / R - TANK2);
		J9 = V / 120 * COSK3 * COSK2;
		J9 = J9 * (5 - 18 * TANK2 + TANK2 * TANK2 + 14 * H2 - 58 * TANK2 * H2);
		East = E0 + P * J7 + P2 * P * J8 + P4 * P * J9;
		IEast = (int) Math.floor(East + 0.5);
		INorth = (int) Math.floor(North + 0.5); // should strictly be trunc
		res[0] = INorth;
		res[1] = IEast;
		return res;
	}

	public static Coordinates molodensky(Coordinates old, Datum from, Datum to) {
		double oldLat, oldLong, oldAlt, lat, lon, alt;
		double dx, dy, dz, da, df, bda, Rn, Rm, deltaLat, deltaLong, deltaAlt;
		oldLat = old.getLatitude();
		oldLong = old.getLongitude();
		if (!Float.isNaN(old.getAltitude())) {
			oldAlt = old.getAltitude();
		} else {
			oldAlt = 0;
		}
		dx = from.dx - to.dx;
		dy = from.dy - to.dy;
		dz = from.dz - to.dz;
		da = to.a - from.a;
		df = to.f - from.f;
		bda = 1.0 - from.f;
		Rn = from.a
				/ Math.sqrt(1 - from.e2 * Math.sin(oldLat * Deg2Rad)
						* Math.sin(oldLat * Deg2Rad));
		Rm = Rn
				* (1 - from.e2)
				/ (1 - from.e2 * Math.sin(oldLat * Deg2Rad)
						* Math.sin(oldLat * Deg2Rad));
		deltaLat = ((((-dx * Math.sin(oldLat * Deg2Rad)
				* Math.cos(oldLong * Deg2Rad) - dy * Math.sin(oldLat * Deg2Rad)
				* Math.sin(oldLong * Deg2Rad)) + dz
				* Math.cos(oldLat * Deg2Rad)) + da * Rn * from.e2
				* Math.sin(oldLat * Deg2Rad) * Math.cos(oldLat * Deg2Rad)
				/ from.a) + df * (Rm / bda + Rn * bda)
				* Math.sin(oldLat * Deg2Rad) * Math.cos(oldLat * Deg2Rad))
				/ (Rm + oldAlt);
		deltaLong = (-dx * Math.sin(oldLong * Deg2Rad) + dy
				* Math.cos(oldLong * Deg2Rad))
				/ ((Rn + oldAlt) * Math.cos(oldLat * Deg2Rad));
		deltaAlt = dx * Math.cos(oldLat * Deg2Rad)
				* Math.cos(oldLong * Deg2Rad) + dy * Math.cos(oldLat * Deg2Rad)
				* Math.sin(oldLong * Deg2Rad) + dz * Math.sin(oldLat * Deg2Rad)
				- da * from.a / Rn + df * bda * Rn * Math.sin(oldLat * Deg2Rad)
				* Math.sin(oldLat * Deg2Rad);

		lat = oldLat + deltaLat / Deg2Rad;
		lon = oldLong + deltaLong / Deg2Rad;
		alt = oldAlt + deltaAlt;
		Coordinates res = new Coordinates(lat, lon, (float) alt);
		return res;
	}

	public static String[] strings(Coordinates point, int grid) {
		String[] res = new String[2];
		if (grid == LatLon) {
			Coordinates coord = dtmConv(point, wgs84, ed50);
			double lat = coord.getLatitude();
			double lon = coord.getLongitude();
			res[0] = (int) Math.floor(lat) + "\u00b0"
					+ (int) Math.floor((lat - Math.floor(lat)) * 60) + "'"
					+ Math.floor((lat - Math.floor(60 * lat) / 60) * 3600)
					+ "\"";
			res[1] = (int) Math.floor(lon) + "\u00b0"
					+ (int) Math.floor((lon - Math.floor(lon)) * 60) + "'"
					+ Math.floor((lon - Math.floor(60 * lon) / 60) * 3600)
					+ "\"";
		} else {
			int[] p = latLon2XY(point, grid);
			res[0] = "" + p[0];
			res[1] = "" + p[1];
		}
		return res;
	}

	public static String toString(Coordinates point, int grid) {
		String[] temp = strings(point, grid);
		String res = temp[1] + "/" + temp[0];
		return res;
	}

	public static Coordinates XY2LatLon(int[] p, int datum) {
		double F0, A1 = 0, B1 = 0, K0 = 0, N0 = 0, N1, N2, N3, E2, E0 = 0, L0 = 0, K, L, SINK, COSK, TANK, TANK2, COSK2, COSK3, K3, K4, MeridWest, Merid, J1, J2, J3, J4, J5, J6, J7, J8, J9, M, Temp, V, R, H2, P, P2, P4, North, East;
		double lat, lon, tol, V1, V2, V3, V5, V7, SECK, TANK4, TANK6, ET, ET2, ET3, ET4, ET5, ET6, ET7;

		North = p[0];
		East = p[1];
		if (datum == UTM) { // UTM International Ellipsoid
			F0 = 0.9996;
			A1 = 6378388.000 * F0;
			B1 = 6356911.946 * F0;
			K0 = 0;
			L0 = 33.0 * Deg2Rad; // <-------
			N0 = 0;
			E0 = 500000;
		} else if (datum == NewITM) {
			F0 = 1.000006700000000; // Local scale factor on Central Meridian
			A1 = 6378300 * F0; // Major Semi-axis, Airy Ellipsoid
			B1 = 6356566 * F0; // Minor Semi-axis, Airy Ellipsoid
			K0 = 31.73439361 * Deg2Rad; // Lat of True Origin
			Merid = 35.20451694;
			L0 = Merid * Deg2Rad; // Long of True Origin (2W)
			N0 = 626907.389999999900000; // Grid Northing of True Origin (m)
			E0 = 219529.584000000000000; // Grid Easting of True Origin (m)
		} else if (datum == OldITM) {
			F0 = 1.000006700000000; // Local scale factor on Central Meridian
			A1 = 6378300 * F0; // Major Semi-axis, Airy Ellipsoid
			B1 = 6356566 * F0; // Minor Semi-axis, Airy Ellipsoid
			K0 = 31.73439361 * Deg2Rad; // Lat of True Origin
			Merid = 35.20451694;
			L0 = Merid * Deg2Rad; // Long of True Origin (2W)
			N0 = 1126907.389999999900000; // Grid Northing of True Origin (m)
			E0 = 169529.584000000000000; // Grid Easting of True Origin (m)
		}

		N1 = (A1 - B1) / (A1 + B1); // n
		N2 = N1 * N1;
		N3 = N2 * N1;
		E2 = ((A1 * A1) - (B1 * B1)) / (A1 * A1); // e^2

		K = (North - N0) / A1 + K0;
		K3 = K - K0;
		K4 = K + K0;

		// ArcofMeridian
		J3 = K3 * (1 + N1 + 1.25 * (N2 + N3));
		J4 = Math.sin(K3) * Math.cos(K4) * (3 * (N1 + N2 + 0.875 * N3));
		J5 = Math.sin(2 * K3) * Math.cos(2 * K4) * (1.875 * (N2 + N3));
		J6 = Math.sin(3 * K3) * Math.cos(3 * K4) * 35 / 24 * N3;
		M = (J3 - J4 + J5 - J6) * B1;

		while (Math.abs(North - N0 - M) > 0.0001) {
			K = (North - N0 - M) / A1 + K;
			K3 = K - K0;
			K4 = K + K0;

			// ArcofMeridian
			J3 = K3 * (1 + N1 + 1.25 * (N2 + N3));
			J4 = Math.sin(K3) * Math.cos(K4) * (3 * (N1 + N2 + 0.875 * N3));
			J5 = Math.sin(2 * K3) * Math.cos(2 * K4) * (1.875 * (N2 + N3));
			J6 = Math.sin(3 * K3) * Math.cos(3 * K4) * 35 / 24 * N3;
			M = (J3 - J4 + J5 - J6) * B1;
		}
		SINK = Math.sin(K);
		COSK = Math.cos(K);
		SECK = 1.0 / COSK;
		TANK = SINK / COSK;
		TANK2 = TANK * TANK;
		TANK4 = TANK2 * TANK2;
		TANK6 = TANK4 * TANK2;
		COSK2 = COSK * COSK;
		COSK3 = COSK2 * COSK;

		// VRH2
		Temp = 1.0 - E2 * SINK * SINK;
		V = A1 / Math.sqrt(Temp);
		R = V * (1 - E2) / Temp;
		H2 = V / R - 1.0;
		V2 = V * V;
		V3 = V * V2;
		V5 = V3 * V2;
		V7 = V5 * V2;
		ET = East - E0;
		ET2 = ET * ET;
		ET3 = ET * ET2;
		ET4 = ET2 * ET2;
		ET5 = ET3 * ET2;
		ET6 = ET4 * ET2;
		ET7 = ET5 * ET2;

		J1 = TANK / (2 * R * V);
		J2 = TANK / (24 * R * V3) * (5 + 3 * TANK2 + H2 - 9 * TANK2 * H2);
		J3 = TANK / (720 * R * V5) * (61 + 90 * TANK2 + 45 * TANK4);
		K = K - ET2 * J1 + ET4 * J2 - ET6 * J3;

		J4 = SECK / V;
		J5 = SECK / (6 * V3) * (V / R + 2 * TANK2);
		J6 = SECK / (120 * V5) * (5 + 28 * TANK2 + 24 * TANK4);
		J7 = SECK / (5040 * V7)
				* (61 + 662 * TANK2 + 1320 * TANK4 + 720 * TANK6);
		L = L0 + ET * J4 - ET3 * J5 + ET5 * J6 - ET7 * J7;

		lat = K / Deg2Rad;
		lon = L / Deg2Rad;
		Coordinates res = new Coordinates(lat, lon, 0);
		Coordinates coord = new Coordinates(0, 0, 0);
		if (datum == UTM) {
			coord = dtmConv(res, ed50, wgs84);
		} else if ((datum == NewITM) | (datum == OldITM)) {
			coord = molodensky(res, grs80, wgs84);
		}

		return coord;
	}

}
